#ifndef VEHICLEA_HPP
#define VEHICLEA_HPP

#include "vehicle.hpp"


class VehicleA : public Vehicle
{
public:

    DetectServer detectServer;

    VehicleA()
        : Vehicle()
    {

    }

    static bool HumanFollowSpeed(cv::Vec3d &speed, DetectResultS &result, cv::Vec2d speedVector, double angleThreshold)
    {
        if(result.human.empty())
        {
            std::cout<<"no human\n"<<std::flush;
            return false;
        }

        std::vector<double> cosAngle(result.human.size(),0);

        for(size_t i=0;i<result.human.size();i++)
        {
            cv::Vec2d humanMove = (result.human[i].speed[0], result.human[i].speed[1]);

            cosAngle[i]=speedVector.ddot(humanMove)/cv::norm(speedVector)/cv::norm(humanMove);
            std::cout<<"angle is "<<cosAngle[i]<<"\n"<<std::flush;
        }

        size_t maxIndex=std::max_element(cosAngle.begin(), cosAngle.end())-cosAngle.begin();

        double cosAngleThreshold=cos(angleThreshold);

        std::cout<<"cosAngle maxindex "<<cosAngle[maxIndex]<<"\n"<<std::flush;
        std::cout<<"detect angle "<<cosAngleThreshold<<"\n"<<std::flush;

        if(cosAngle[maxIndex]<cosAngleThreshold)
        {
            std::cout<<"no human with direction\n"<<std::flush;
            return false;
        }

        cv::Vec2d location=result.human[maxIndex].XY();
        std::cout<<"follow human at "<<location<<", id="<<result.human[maxIndex].objID<<"\n"<<std::flush;
        speed=Follower::ToSpeed(location[0], location[1]);

        return true;

    }



    static bool HumanFollowSpeed(cv::Vec3d &speed, DetectResultS &result, double ySpeedThreshold)
    {
        if(result.human.empty())
        {
            std::cout<<"no human\n"<<std::flush;
            return false;
        }

        size_t minXindex=result.human.size();

        for(size_t i=0;i<result.human.size();i++)
        {
           if(result.human[i].speed[1]>ySpeedThreshold)
           {
               if(minXindex<result.human.size())
               {
                   if(fabs(result.human[minXindex].xzlocation[0])>fabs(result.human[i].xzlocation[0]))
                       minXindex=i;
               }
               else
               {
                   minXindex=i;
               }
           }
        }


        if(minXindex<result.human.size())
        {
            cv::Vec2d location=result.human[minXindex].XY();
            std::cout<<"follow human at "<<location<<", id="<<result.human[minXindex].objID<<"\n"<<std::flush;
            speed=Follower::ToSpeed(location[0], location[1]);

            return true;

        }

        std::cout<<"no human within y speed threshold\n"<<std::flush;
        return false;

    }



    static void Control(DoubleBf2<AnalysisResultA> &in, DoubleBf2<DetectResultS> &in2, DoubleBf2<AnalysisResultA> &out, cv::Vec2d goal=cv::Vec2d(0,650), double targetRad=0)
    {
        float distancethresh = 30;
        double propertyThres=0.3;

        float radThres=CV_PI/90;

        float directionThres=CV_PI*180/180;

        ToST tst;

        tst.Initial();

        Dstar plan;

        plan.init(0,0,goal[0],goal[1]);         // set start to (0,0) and goal to (10,5)

        AnalysisResultA *par=NULL;

        cv::Vec3d speedOut(0,0,0);

        cv::Vec2d goalAtCar(0,1);

        DetectResultS useResult;

        while(true)
        {
            in.Read(par);

            if(par!=NULL)
            {
                cv::Vec3d speed(0,0,0);
                if(par->property<=propertyThres)
                {
                    std::cout<<"no target stop !"<<"\n";
                    std::cout<<"last goal at "<<goalAtCar<<"\n"<<std::flush;
                    if(goalAtCar[0]<0)
                        speed=cv::Vec3d(0,0,0.1);
                    else
                        speed=cv::Vec3d(0,0,-0.1);
                }
                else
                {


                    goalAtCar=par->ToCarCoordinate(goal);

                    double dfRad=par->location[2]-targetRad;
                    FixRadDiff(dfRad);

                    if(abs(goalAtCar[0])<distancethresh && abs(goalAtCar[1])<distancethresh && abs(dfRad)<radThres)
                    {
                        std::cout<<"reached"<<"\n"<<std::flush;
                        break;
                    }
                    else
                    {


                        cv::Vec3f v=ComputeSpeedA(goalAtCar, plan, dfRad);

                        std::cout<<"v="<<v<<"\n"<<std::flush;

                        speed=cv::Vec3d(v[0],v[1],v[2]);

                        if(cv::norm(goalAtCar)>1000)
                        {


#if 1
                            DetectResultS *pResult1=NULL;
                            in2.Read(pResult1);

                            par->drs=(*pResult1);



                            cv::Vec3d hfs;
                            std::cout<<"check human"<<"\n"<<std::flush;

                            if(HumanFollowSpeed(hfs, *pResult1, cv::Vec2d(speed[0],speed[1]), directionThres))
                            {
                                speed=hfs;
                                par->isFollowing=true;
                            }
                            else
                            {
                                par->isFollowing=false;
                                //                            useResult.human.clear();
                                std::cout<<"clear useresult\n"<<std::flush;
                            }

                            delete pResult1;

#endif

#if 0
                            DetectResultS *pResult1=NULL;
                            in2.Read(pResult1);

                            par->drs=(*pResult1);

                            if(!pResult1->human.empty())
                            {
                                useResult=(*pResult1);

                                std::cout<<"update useresult\n"<<std::flush;
                            }
                            delete pResult1;

                            cv::Vec3d hfs;
                            std::cout<<"check human"<<"\n"<<std::flush;

                            if(HumanFollowSpeed(hfs, useResult, cv::Vec2d(speed[0],speed[1]), directionThres))
                            {
                                speed=hfs;
                                par->isFollowing=true;
                            }
                            else
                            {
                                par->isFollowing=false;
                                useResult.human.clear();
                                std::cout<<"clear useresult\n"<<std::flush;
                            }



#endif

                        }


                    }



                }

#if 0
                DetectResultS *pResult1=NULL;
                in2.Read(pResult1);
                par->drs=(*pResult1);
                delete pResult1;
#endif


                Follower::LowPass(speedOut, speed);

                std::cout<<"speedOut="<<speedOut<<"\n"<<std::flush;

                tst.Move(speedOut[0],speedOut[1],speedOut[2]);

                out.Write(par);
            }
            else
            {
                std::cout<<"null pm\n"<<std::flush;
                break;
            }
            std::cout.flush();
        }

        par=NULL;
        out.Write(par);

        tst.Move();

        tst.End();

        std::cout<<"end Control\n"<<std::flush;
    }


    static void Control2(DoubleBf2<AnalysisResultA> &in, DoubleBf2<DetectResultS> &in2, DoubleBf2<AnalysisResultA> &out, cv::Vec2d goal=cv::Vec2d(0,650), double targetRad=0)
    {
        float distancethresh = 30;
        double propertyThres=0.3;

        float radThres=CV_PI/90;

//        float directionThres=CV_PI*180/180;

        float ySpeedThres=-0.3;

        ToST tst;

        tst.Initial();

        Dstar plan;

        plan.init(0,0,goal[0],goal[1]);         // set start to (0,0) and goal to (10,5)

        AnalysisResultA *par=NULL;

        cv::Vec3d speedOut(0,0,0);

        cv::Vec2d goalAtCar(0,1);

        while(true)
        {
            in.Read(par);

            if(par!=NULL)
            {
                cv::Vec3d speed(0,0,0);
                if(par->property<=propertyThres)
                {
                    std::cout<<"no target stop !"<<"\n";


                    DetectResultS *pResult1=NULL;
                    in2.Read(pResult1);

                    par->drs=(*pResult1);

                    cv::Vec3d hfs;
                    std::cout<<"check human"<<"\n"<<std::flush;

                    if(HumanFollowSpeed(hfs, *pResult1, ySpeedThres))
                    {
                        speed=hfs;
                        par->isFollowing=true;
                    }
                    else
                    {
                        if(goalAtCar[0]<0)
                            speed=cv::Vec3d(0,0,0.1);
                        else
                            speed=cv::Vec3d(0,0,-0.1);


                        par->isFollowing=false;
                        std::cout<<"clear useresult\n"<<std::flush;
                    }

                    delete pResult1;



                }
                else
                {


                    goalAtCar=par->ToCarCoordinate(goal);

                    double dfRad=par->location[2]-targetRad;
                    FixRadDiff(dfRad);

                    if(abs(goalAtCar[0])<distancethresh && abs(goalAtCar[1])<distancethresh && abs(dfRad)<radThres)
                    {
                        std::cout<<"reached"<<"\n"<<std::flush;
                        break;
                    }
                    else
                    {


                        cv::Vec3f v=ComputeSpeedA(goalAtCar, plan, dfRad);

                        std::cout<<"v="<<v<<"\n"<<std::flush;

                        speed=cv::Vec3d(v[0],v[1],v[2]);

                        if(cv::norm(goalAtCar)>1000)
                        {

                            DetectResultS *pResult1=NULL;
                            in2.Read(pResult1);

                            par->drs=(*pResult1);

                            cv::Vec3d hfs;
                            std::cout<<"check human"<<"\n"<<std::flush;

                            if(HumanFollowSpeed(hfs, *pResult1, ySpeedThres))
                            {
                                speed=hfs;
                                par->isFollowing=true;
                            }
                            else
                            {
                                par->isFollowing=false;
                                std::cout<<"clear useresult\n"<<std::flush;
                            }

                            delete pResult1;


                        }
                    }
                }




                Follower::LowPass(speedOut, speed);

                std::cout<<"speedOut="<<speedOut<<"\n"<<std::flush;

                tst.Move(speedOut[0],speedOut[1],speedOut[2]);

                out.Write(par);
            }
            else
            {
                std::cout<<"null pm\n"<<std::flush;
                break;
            }
            std::cout.flush();
        }

        par=NULL;
        out.Write(par);

        tst.Move();

        tst.End();

        std::cout<<"end Control\n"<<std::flush;
    }


    void StartMove()
    {
		R180();
		std::this_thread::sleep_for(std::chrono::milliseconds(500));
        Y(2500);        
    }


    virtual void DisplayC(std::string command, cv::Vec2d goal, double goalRad)
    {
        StartMove();


        cv::namedWindow(command,cv::WINDOW_NORMAL);
        cv::moveWindow(command,32,32);
        cv::resizeWindow(command, 1280, 720);

        DoubleBf2<AnalysisResultA> bf2;
        DoubleBf2<AnalysisResultA> bf3;

        std::thread th2(AnalyzeThread, this, std::ref(bf2));
        std::thread th3(Control2, std::ref(bf2), std::ref(detectServer.humanBuffer), std::ref(bf3), goal, goalRad);

        while(true)
        {
            AnalysisResultA *par=NULL;
            bf3.Read(par);
            if(par!=NULL)
            {
                par->Draw();
                cv::imshow(command, *(par->frame));
                k=cv::waitKey(10);
                delete par;
            }
            else
            {
                break;
            }
        }

        play=false;

        std::this_thread::sleep_for(std::chrono::milliseconds(500));

        cv::Mat* pm=NULL;

        frameBuffer.Write(pm);

        if(th2.joinable()) th2.join();

        if(th3.joinable()) th3.join();

        cv::destroyWindow(command);

        std::cout<<"end DisplayC\n"<<std::flush;

    }


    void Start(uint16_t portno=7654)
    {
        detectServer.Start(4567);
        Vehicle::start(portno);
        detectServer.End();
    }


};



#endif // VEHICLEA_HPP

